#include "stdafx.h"
#include "cvxGeometry.h"
#include <algorithm>
#include <iostream>
#include <queue>
#include <map>
//#include "cvxSf.h"


using std::cout;
using std::cerr;
using std::endl;
using std::queue;
using std::map;
using cvx::BwImage;
using cvx::BwImageInt;
using cvx::BwImageFloat;
using cvx::Bw32FImage;
using cvx::Hsv8UImage;


static void HOUSEHOLDER(CvMat* A, CvMat* V)
{
	int n = A->rows;
	cvCopy(A, V);

	double beta;
	if (cvmGet(A,0,0) >= 0)
	{
		beta = cvmGet(A,0,0) + cvNorm(A);
	}
	else
	{
		beta = cvmGet(A,0,0) - cvNorm(A);
	}

	cvmSet(V,0,0,1);
	for (int i = 1; i < n; i++)
	{
		cvmSet(V, i, 0, 1/beta*cvmGet(V, i, 0));
	}
}

static void HOUSEHOLDER_MULT(CvMat* A, CvMat* V)
{
	int n = V->rows;
	CvMat* beta0 = cvCreateMat(1,1,CV_64FC1);

	cvMulTransposed(V, beta0, 1);

	double beta = -2/cvmGet(beta0,0,0);
	cvReleaseMat(&beta0);

	CvMat* w = cvCreateMat(1, A->cols, CV_64FC1);
	CvMat* VT = cvCreateMat(V->cols, V->rows, CV_64FC1);
	cvTranspose(V, VT);
	cvMatMul(VT,A,w);
	cvReleaseMat(&VT);

	cvGEMM(V,w,beta,A,1,A,0);
	cvReleaseMat(&w);
}

static void QR_HOUSE(CvMat* A)
{
	int n = A->rows;
	int m = A->cols;
	int i, j;
	int h, g;

	for (int k = 0; k < std::min(n-1, m); k++)
	{
		CvMat* A0 = cvCreateMat(n-k,1,CV_64FC1);
		CvMat* V = cvCreateMat(n-k,1,CV_64FC1);

		j = 0;
		for (i = k; i < n; i++)
		{
			cvmSet(A0,j,0,cvmGet(A,i,k));
			j++;
		}

		HOUSEHOLDER(A0, V);
		cvReleaseMat(&A0);

		CvMat* A1 = cvCreateMat(n-k,m-k,CV_64FC1);
		for (i = 0, h = k; i < n-k, h < n; i++, h++)
		{
			for (j = 0, g = k; j < m-k, g < m; j++, g++)
			{
				cvmSet(A1,i,j,cvmGet(A,h,g));
			}
		}
		HOUSEHOLDER_MULT(A1,V);

		for (i = 0, h = k; i < n-k, h < n; i++, h++)
		{
			for (j = 0, g = k; j < m-k, g < m; j++, g++)
			{
				cvmSet(A,h,g,cvmGet(A1,i,j));
			}
		}

		cvReleaseMat(&A1);

		for (i = k+1, j = 1; i < n, j < n-k; i++, j++)
		{
			cvmSet(A,i,k,cvmGet(V,j,0));
		}

		cvReleaseMat(&V);
	}
}

static void Q_HOUSE(CvMat* A, CvMat* Q)
{
	int i, j;
	int g, h;
	int n = A->rows;
	cvSetIdentity(Q);

	for (int k = 0; k < n-1; k++)
	{
		CvMat* V = cvCreateMat(n-k,1,CV_64FC1);
		cvSet(V, cvScalarAll(1), NULL);

		for (i = 1, j = k+1; i < n-k, j < n; i++, j++)
		{
			cvmSet(V,i,0,cvmGet(A,j,k));
		}

		CvMat* Qk = cvCreateMat(n,n,CV_64FC1);
		cvSetIdentity(Qk);
		CvMat* II = cvCreateMat(n-k,n-k,CV_64FC1);
		cvSetIdentity(II);
		CvMat* VT = cvCreateMat(V->cols,V->rows,CV_64FC1);
		CvMat* de = cvCreateMat(1,1,CV_64FC1);
		cvTranspose(V,VT);
		cvMatMul(VT,V,de);
		CvMat* QQ = cvCreateMat(n-k,n-k,CV_64FC1);
		cvGEMM(V,VT,-2/cvmGet(de,0,0),II,1,QQ);

		cvReleaseMat(&V);
		cvReleaseMat(&VT);
		cvReleaseMat(&de);
		cvReleaseMat(&II);

		for (i = k, g = 0; i < n, g < QQ->rows; i++, g++)
		{
			for (j = k, h = 0; j < n, h < QQ->cols; j++, h++)
			{
				cvmSet(Qk,i,j,cvmGet(QQ,g,h));
			}
		}
		cvReleaseMat(&QQ);

		CvMat* Q0 = cvCreateMat(n,n,CV_64FC1);
		cvMatMul(Qk,Q,Q0);
		cvCopy(Q0, Q);

		cvReleaseMat(&Qk);
		cvReleaseMat(&Q0);
	}
}


static void qrHouse(CvMat* A, CvMat* Q, CvMat* R)
{
	CvMat* B = cvCreateMat(A->rows, A->cols, CV_64FC1);
	cvCopy(A, B);
	QR_HOUSE(B);

	CvMat* QT = cvCreateMat(Q->cols,Q->rows,CV_64FC1);
	Q_HOUSE(B, QT);

	cvMatMul(QT, A, R);
	cvTranspose(QT, Q);

	cvReleaseMat(&B);
	cvReleaseMat(&QT);
}


void CvxGeometry::KRT_From_P_QR(CvMat* P, CvMat* K, CvMat* R, CvMat* T)
{
	int i,j;

	//step one
	CvMat* H = cvCreateMat(3,3,CV_64FC1);
	CvMat* h = cvCreateMat(3,1,CV_64FC1);
	for (i = 0; i < 3; i++)
	{
		for (j = 0; j < 3; j++)
		{
			cvmSet(H,i,j,cvmGet(P,i,j));
		}
		cvmSet(h,i,0,cvmGet(P,i,3));
	}


	//step two
	CvMat* Hinv = cvCreateMat(3,3,CV_64FC1);
	cvInvert(H,Hinv,CV_SVD);
	CvMat* Qd   = cvCreateMat(H->rows,H->cols,CV_64FC1);
	CvMat* Rd   = cvCreateMat(H->rows,H->rows,CV_64FC1);

	cvReleaseMat(&H);
	qrHouse(Hinv,Qd,Rd);
	cvInvert(Rd,K,CV_SVD);
	cvTranspose(Qd,R);
	cvReleaseMat(&Hinv);
	cvReleaseMat(&Qd);

	//step three
	double s1[9] = {-1,0,0,0,1,0,0,0,-1};
	double s2[9] = {-1,0,0,0,-1,0,0,0,1};
	double s3[9] = {1,0,0,0,-1,0,0,0,-1};
	double s4[9] = {1,0,0,0,1,0,0,0,1};

	CvMat* Rn = cvCreateMat(3,3,CV_64FC1);
	CvMat* Kn = cvCreateMat(3,3,CV_64FC1);
	CvMat* S1 = cvCreateMat(3,3,CV_64FC1);
	CvMat* S2 = cvCreateMat(3,3,CV_64FC1);
	CvMat* S3 = cvCreateMat(3,3,CV_64FC1);
	CvMat* S4 = cvCreateMat(3,3,CV_64FC1);
	cvInitMatHeader(S1,3,3,CV_64FC1,s1);
	cvInitMatHeader(S2,3,3,CV_64FC1,s2);
	cvInitMatHeader(S3,3,3,CV_64FC1,s3);
	cvInitMatHeader(S4,3,3,CV_64FC1,s4);

	double a = cvmGet(K,0,0);
	double b = cvmGet(K,1,1);
	double c = cvmGet(K,2,2);

	CvMat* t0 = cvCreateMat(3,3,CV_64FC1);

	if ((a>0 && b<0 && c>0) || (a<0 && b>0 && c<0))
	{
		cvMatMul(K,S1,Kn);
		cvMatMul(S1,R,Rn);
		cvMatMul(S1,Rd,t0);
	}
	else if ((a>0 && b>0 && c<0) || (a<0 && b<0 && c>0))
	{
		cvMatMul(K,S2,Kn);
		cvMatMul(S2,R,Rn);
		cvMatMul(S2,Rd,t0);
	}
	else if ((a>0 && b<0 && c<0) || (a<0 && b>0 && c>0))
	{
		cvMatMul(K,S3,Kn);
		cvMatMul(S3,R,Rn);
		cvMatMul(S2,Rd,t0);
	}
	else if ((a>0 && b>0 && c>0) || (a<0 && b<0 && c<0))
	{
		cvMatMul(K,S4,Kn);
		cvMatMul(S4,R,Rn);
		cvMatMul(S2,Rd,t0);
	}

	cvMatMul(t0,h,T);

	cvReleaseMat(&t0);
	cvReleaseMat(&S1);
	cvReleaseMat(&S2);
	cvReleaseMat(&S3);
	cvReleaseMat(&S4);
	cvReleaseMat(&Rd);
	cvReleaseMat(&h);

	for (i = 0; i < 3; i++)
	{
		for (j = 0; j < 3; j++)
		{
			cvmSet(K,i,j,cvmGet(Kn,i,j)/cvmGet(Kn,2,2));
		}
	}
	cvReleaseMat(&Kn);
	cvCopy(Rn,R);
	cvReleaseMat(&Rn);
}

void CvxGeometry::KRT_From_P_QR(float* f_P, float *f_K, float *f_R, float *f_T)
{
	//decompose matrix
	CvMat *P = cvCreateMat(3, 4, CV_64FC1);
	CvMat *K = cvCreateMat(3, 3, CV_64FC1);
	CvMat *R = cvCreateMat(3, 3, CV_64FC1);
	CvMat *T = cvCreateMat(3, 1, CV_64FC1);
	cvx::MatDouble p_mat(P);
	for (int y = 0; y<3; ++y) {
		for (int x = 0; x<4; ++x) {
			int index = y * 4 + x;
			*p_mat(y, x) = f_P[index];
		}
	}
	CvxGeometry::KRT_From_P_QR(P, K, R, T);
	
	cvx::MatDouble k_mat(K);
	cvx::MatDouble r_mat(R);
	cvx::MatDouble t_mat(T);
	for (int y = 0; y<3; ++y) {
		f_T[y] = *t_mat(y, 0);
		for (int x = 0; x<3; ++x) {
			int i = y * 3 + x;
			f_K[i] = *k_mat(y, x);
			f_R[i] = *r_mat(y, x);
		}
	}	
	cvReleaseMat(&P);
	cvReleaseMat(&K);
	cvReleaseMat(&R);
	cvReleaseMat(&T);
}

void CvxGeometry::GetEulerAngleInSphereCoor(float x, float y, float z, float &theta, float &phi)
{
	assert(fabs(x * x + y * y + z * z - 1.0) < 0.000001);
	assert(x*x + y*y > 0);

	theta = acos(z/sqrt(1.0*x * x + y * y + z * z));
	if (y >= 0)
	{
		phi = acos(x/sqrt(1.0*x*x + y*y));
	}
	else
	{
		phi = 2 * CV_PI - acos(x/sqrt(1.0*x*x + y*y));
	}
}

void CvxGeometry::GetOrientation(const Mat &rvec, const Mat &tvec, const float unitLength, const int w, const int h,
					float &theta, float &phi, float &z_distance)
{
	assert(rvec.cols == 1 && rvec.rows == 3);
	assert(tvec.cols == 1 && tvec.rows == 3);

	//rodrigues vector to rotation matrix
	double r33[9];
	double p0[] = {0, 0, 0};
	double p1[] = {unitLength*w, unitLength*h, 0};
	double p2[] = {0, unitLength*h, 0};	
	cv::Mat rMat(3, 3, CV_64F, r33);
	cv::Rodrigues(rvec, rMat);

	//normal of plane z + 1 = 0; as initial model plane
	cv::Mat p0_mat(3, 1, CV_64F, p0);
	cv::Mat p1_mat(3, 1, CV_64F, p1);
	cv::Mat p2_mat(3, 1, CV_64F, p2);
	p0_mat = rMat * p0_mat;
	p1_mat = rMat * p1_mat;
	p2_mat = rMat * p2_mat;

	Mat p01 = p1_mat - p0_mat;
	Mat p02 = p2_mat - p0_mat;
	Mat ori = p02.cross(p01);   // ori is normal of plane after translation

	float x = ori.at<double>(0, 0);
	float y = ori.at<double>(1, 0);
	float z = ori.at<double>(2, 0);
	float len = 1.0/sqrt(1.0 *x*x + y*y + z*z);
	x *= len; 
	y *= len;
	z *= len;

	CvxGeometry::GetEulerAngleInSphereCoor(x, y, z, theta, phi);

	theta *= 180/CV_PI;
	phi   *= 180/CV_PI;

	//model distance in z direction
	double center[] = {unitLength * w/2, unitLength * h/2, 0};
	Mat center_mat = Mat(3, 1, CV_64F, center);
	center_mat = rMat * center_mat;
	center_mat = tvec + center_mat;
	z_distance = center_mat.at<double>(2, 0);
}

 void CvxGeometry::GetPlaneNormalAndPatternCenterPosition(const Mat &rvec, const Mat &tvec, const float unitLength, 
								   const CvSize &patternSize, Vec3d &normal, Point3f &pos)
 {
	 assert(rvec.cols == 1 && rvec.rows == 3);
	 assert(tvec.cols == 1 && tvec.rows == 3);

	 //rodrigues vector to rotation matrix
	 double r33[9];
	 double p0[] = {0, 0, 0};
	 double p1[] = {unitLength*patternSize.width, unitLength*patternSize.height, 0};
	 double p2[] = {0, unitLength*patternSize.height, 0};	
	 cv::Mat rMat(3, 3, CV_64F, r33);
	 cv::Rodrigues(rvec, rMat);

	 //normal of plane z + 1 = 0; as initial model plane
	 cv::Mat p0_mat(3, 1, CV_64F, p0);
	 cv::Mat p1_mat(3, 1, CV_64F, p1);
	 cv::Mat p2_mat(3, 1, CV_64F, p2);
	 p0_mat = rMat * p0_mat;
	 p1_mat = rMat * p1_mat;
	 p2_mat = rMat * p2_mat;

	 Mat p01 = p1_mat - p0_mat;
	 Mat p02 = p2_mat - p0_mat;
	 Mat ori = p02.cross(p01);   // ori is normal of plane after translation

	 double x = ori.at<double>(0, 0);
	 double y = ori.at<double>(1, 0);
	 double z = ori.at<double>(2, 0);
	 double len = 1.0/sqrt(1.0 *x*x + y*y + z*z);
	 x *= len; 
	 y *= len;
	 z *= len;
	 normal = Vec3d(x, y, z);

	 //model distance in z direction
	 double center[] = {unitLength * patternSize.width/2, unitLength * patternSize.height/2, 0};
	 Mat center_mat = Mat(3, 1, CV_64F, center);
	 center_mat = rMat * center_mat + tvec;
	 pos.x = center_mat.at<double>(0, 0);
	 pos.y = center_mat.at<double>(1, 0);
	 pos.z = center_mat.at<double>(2, 0);	 
 }

void CvxGeometry::GetEulerAngleRvec(const Mat &rvec, Vec3d &euA, Vec3d &euB)
{
	assert(rvec.rows == 3);
	assert(rvec.cols == 1);
	cv::Mat rMat(3, 3, CV_64F);
	cv::Rodrigues(rvec, rMat);
	CvxGeometry::GetEulerAngle(rMat, euA, euB);

// 	double r11, r12, r13, r21, r22, r23, r31, r32, r33;
// 	r11 = rMat.at<double>(0, 0);
// 	r12 = rMat.at<double>(0, 1);
// 	r13 = rMat.at<double>(0, 2);
// 	r21 = rMat.at<double>(1, 0);
// 	r22 = rMat.at<double>(1, 1);
// 	r23 = rMat.at<double>(1, 2);
// 	r31 = rMat.at<double>(2, 0);
// 	r32 = rMat.at<double>(2, 1);
// 	r33 = rMat.at<double>(2, 2);
// 	if (r31 != 1.0 || r31 != -1.0)
// 	{
// 		euA[1] = -asin(r31);
// 		euB[1] = CV_PI - euA[1];
// 		euA[0] = atan2(r32/cos(euA[1]), r33/cos(euA[1]));
// 		euB[0] = atan2(r32/cos(euB[1]), r33/cos(euB[1]));
// 		euA[2] = atan2(r21/cos(euA[1]), r11/cos(euA[1]));
// 		euB[2] = atan2(r21/cos(euB[1]), r11/cos(euB[1]));
// 	}
// 	else
// 	{
// 		euA[2] = 0;
// 		euB[2] = 0;
// 		if (r31 == -1.0)
// 		{
// 			euA[1] = CV_PI/2;
// 			euA[0] = euA[2] + atan2(r12, r13);
// 			euB[1] = CV_PI/2;
// 			euB[0] = euB[2] + atan2(r12, r13);
// 		}
// 		else
// 		{
// 			euA[1] = -CV_PI/2;
// 			euA[0] = -euA[2] + atan2(-r12, -r13);
// 			euB[1] = -CV_PI/2;
// 			euB[0] = -euB[2] + atan2(-r12, -r13);
// 		}
// 	}
}

void CvxGeometry::GetEulerAngle(const Mat &rMat33, Vec3d &euA, Vec3d &euB)
{
	assert(rMat33.rows == 3);
	assert(rMat33.cols == 3);

	double r11, r12, r13, r21, r22, r23, r31, r32, r33;
	r11 = rMat33.at<double>(0, 0);
	r12 = rMat33.at<double>(0, 1);
	r13 = rMat33.at<double>(0, 2);
	r21 = rMat33.at<double>(1, 0);
	r22 = rMat33.at<double>(1, 1);
	r23 = rMat33.at<double>(1, 2);
	r31 = rMat33.at<double>(2, 0);
	r32 = rMat33.at<double>(2, 1);
	r33 = rMat33.at<double>(2, 2);
	if (r31 != 1.0 || r31 != -1.0)
	{
		euA[1] = -asin(r31);
		euB[1] = CV_PI - euA[1];
		euA[0] = atan2(r32/cos(euA[1]), r33/cos(euA[1]));
		euB[0] = atan2(r32/cos(euB[1]), r33/cos(euB[1]));
		euA[2] = atan2(r21/cos(euA[1]), r11/cos(euA[1]));
		euB[2] = atan2(r21/cos(euB[1]), r11/cos(euB[1]));
	}
	else
	{
		euA[2] = 0;
		euB[2] = 0;
		if (r31 == -1.0)
		{
			euA[1] = CV_PI/2;
			euA[0] = euA[2] + atan2(r12, r13);
			euB[1] = CV_PI/2;
			euB[0] = euB[2] + atan2(r12, r13);
		}
		else
		{
			euA[1] = -CV_PI/2;
			euA[0] = -euA[2] + atan2(-r12, -r13);
			euB[1] = -CV_PI/2;
			euB[0] = -euB[2] + atan2(-r12, -r13);
		}
	}
}

bool CvxGeometry::FitVanishingPoint(const vector<cv::Vec4f> &lineSegments, Point2f &vp)
{
	assert(lineSegments.size() > 4);
	//minimize d = abs((x2-x1)*(y1-y0) - (x1-x0)*(y2-y1)/sqrt((x2-x1)^2+(y2-y1)^2)

	vector<map<int, double>> leftVec;
	vector<double> rightVec;
	for (int i = 0; i<lineSegments.size(); ++i)
	{
		float x1 = lineSegments[i][0];
		float y1 = lineSegments[i][1];
		float x2 = lineSegments[i][2];
		float y2 = lineSegments[i][3];
		float x21 = x2 - x1;
		float y21 = y2 - y1;
		float denominator = sqrt(x21*x21 + y21*y21);
		if (denominator > 0)
		{			
			map<int, double> imap;
			double v_right = 0.0;
			imap[0] = y21/denominator;
			imap[1] = -x21/denominator;
			v_right = -(x21*y1 - y21 * x1)/denominator;
			leftVec.push_back(imap);
			rightVec.push_back(v_right);			
		}
	}
/*
	if (leftVec.size() > 4 && leftVec.size() == rightVec.size())
	{
		vector<double> rst(2);
		bool isOK = CvxSF::LeastSquare(leftVec, rightVec, rst);
		if (isOK)
		{
			vp.x = rst[0];
			vp.y = rst[1];
			return true;
		}
	}
	*/
	printf("CvxGeometry::FitVanishingPoint not complete");

	return false;
}

Mat CvxGeometry::RotateX(const float angle, bool isDegree)
{
	float a = angle;
	if (isDegree)
	{
		a = angle/180*CV_PI;
	}
	Mat ret = Mat::eye(3, 3, CV_64F);
	ret.at<double>(1, 1) = cos(a);
	ret.at<double>(1, 2) = -sin(a);
	ret.at<double>(2, 1) = sin(a);
	ret.at<double>(2, 2) = cos(a);
	return ret;
}

Mat CvxGeometry::RotateY(const float angle, bool isDegree)
{
	float a = angle;
	if (isDegree)
	{
		a = angle/180*CV_PI;
	}
	Mat ret = Mat::eye(3, 3, CV_64F);
	ret.at<double>(0, 0) = cos(a);
	ret.at<double>(0, 2) = sin(a);
	ret.at<double>(2, 0) = -sin(a);
	ret.at<double>(2, 2) = cos(a);
	return ret;
}

Mat CvxGeometry::RotateZ(const float angle, bool isDegree)
{
	float a = angle;
	if (isDegree)
	{
		a = angle/180*CV_PI;
	}
	Mat ret = Mat::eye(3, 3, CV_64F);
	ret.at<double>(0, 0) = cos(a);
	ret.at<double>(0, 1) = -sin(a);
	ret.at<double>(1, 0) = sin(a);
	ret.at<double>(1, 1) = cos(a);
	return ret;
}

Mat CvxGeometry::hsvBand(int h)
{
	int w = 180;
	Mat hsv = cv::Mat(h, w, CV_8UC3);
	
	float ratio = 255.0/h;
	Hsv8UImage hsv_img(&hsv);
	for(int y = 0; y<h; y++)
	{
		for(int x = 0; x<w; x++)
		{
			hsv_img[y][x].h = x;
			hsv_img[y][x].s = (h-y)*ratio;
			hsv_img[y][x].v = 255;
		}
	}

	cv::cvtColor(hsv, hsv, CV_HSV2BGR);
	return hsv;
}




